#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_INA219.h>
#include <Adafruit_SSD1306.h>
#include <Arduino.h>
#define POTENTIOMETER_PIN 34// 使用ESP32的ADC引脚

// 根据规格书定义参数
#define SCREEN_WIDTH 128   // OLED宽度（像素）
#define SCREEN_HEIGHT 64   // OLED高度（像素）
#define OLED_ADDR 0x3C     // I2C地址（规格书第8页）
#define SDA_PIN 19         // ESP32默认SDA
#define SCL_PIN 22

// 修正引脚定义
#define L_EN_PIN_L    5  // 左侧使能
#define R_EN_PIN_L    17  // 左侧使能
#define L_PWM_PIN_L   26  // 左侧正转PWM
#define R_PWM_PIN_L   27  // 左侧反转PWM
#define L_EN_PIN_R    33   // 右侧使能
#define R_EN_PIN_R    25  // 右侧使能
#define L_PWM_PIN_R   16  // 右侧正转PWM
#define R_PWM_PIN_R   4   // 右侧反转PWM
// 使用四个独立PWM通道
#define PWM_CH_L_L 0  // 左电机正转
#define PWM_CH_R_L 1  // 左电机反转
#define PWM_CH_L_R 2  // 右电机正转
#define PWM_CH_R_R 3  // 右电机反转
const int potPinX = 35;
const int potPinY = 32;
int potXValue = 0,potYValue = 0;         // ESP32默认SCL
int batPercent=50,potPercent;
int current=0;
const int freq = 25000;    // 25kHz PWM频率
const int resolution = 8;  // 8位分辨率
int speed = 0;
int direction_val = 2050;  // 摇杆中值

// 创建OLED对象
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  Serial.begin(115200);
  
  // 初始化I2C（根据规格书第7页引脚定义）
  Wire.begin(SDA_PIN, SCL_PIN);
  pinMode(L_EN_PIN_L, OUTPUT);
  pinMode(R_EN_PIN_L, OUTPUT);
  pinMode(L_EN_PIN_R, OUTPUT);
  pinMode(R_EN_PIN_R, OUTPUT);
  digitalWrite(L_EN_PIN_L, HIGH);
  digitalWrite(R_EN_PIN_L, HIGH);
  digitalWrite(L_EN_PIN_R, HIGH);
  digitalWrite(R_EN_PIN_R, HIGH);
  ledcSetup(PWM_CH_L_L, freq, resolution);
  ledcSetup(PWM_CH_R_L, freq, resolution);
  ledcSetup(PWM_CH_L_R, freq, resolution);
  ledcSetup(PWM_CH_R_R, freq, resolution);
  // 正确连接PWM引脚
  ledcAttachPin(L_PWM_PIN_L, PWM_CH_L_L);  // 左正
  ledcAttachPin(R_PWM_PIN_L, PWM_CH_R_L);  // 左反
  ledcAttachPin(L_PWM_PIN_R, PWM_CH_L_R);  // 右正
  ledcAttachPin(R_PWM_PIN_R, PWM_CH_R_R);  // 右反
  
  // 初始化OLED（规格书第9-10页电气特性）
  if(!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println("OLED初始化失败!");
    while(1); // 停止执行
  }
  Serial.println("OLED初始化成功!");


  // 应用规格书第13页的初始化序列
  display.ssd1306_command(SSD1306_DISPLAYOFF);                    // 0xAE
  display.ssd1306_command(SSD1306_SETDISPLAYCLOCKDIV);            // 0xD5
  display.ssd1306_command(0x80);                                  // 建议值
  display.ssd1306_command(SSD1306_SETMULTIPLEX);                  // 0xA8
  display.ssd1306_command(SCREEN_HEIGHT - 1);                     // 0x3F (64-1)
  display.ssd1306_command(SSD1306_SETDISPLAYOFFSET);              // 0xD3
  display.ssd1306_command(0x00);                                  // 无偏移
  display.ssd1306_command(SSD1306_SETSTARTLINE | 0x00);           // 0x40
  display.ssd1306_command(SSD1306_CHARGEPUMP);                    // 0x8D
  display.ssd1306_command(0x14);                                  // 启用电荷泵
  display.ssd1306_command(SSD1306_MEMORYMODE);                    // 0x20
  display.ssd1306_command(0x00);                                  // 水平寻址模式
  display.ssd1306_command(SSD1306_SEGREMAP | 0x01);               // 0xA1
  display.ssd1306_command(SSD1306_COMSCANDEC);                    // 0xC8
  display.ssd1306_command(SSD1306_SETCOMPINS);                    // 0xDA
  display.ssd1306_command(0x12);                                  // 引脚配置
  display.ssd1306_command(SSD1306_SETCONTRAST);                   // 0x81
  display.ssd1306_command(0xCF);                                  // 对比度值
  display.ssd1306_command(SSD1306_SETPRECHARGE);                  // 0xD9
  display.ssd1306_command(0xF1);                                  // 预充电周期
  display.ssd1306_command(SSD1306_SETVCOMDETECT);                 // 0xDB
  display.ssd1306_command(0x30);                                  // Vcomh电平
  display.ssd1306_command(SSD1306_DISPLAYALLON_RESUME);           // 0xA4
  display.ssd1306_command(SSD1306_NORMALDISPLAY);                 // 0xA6
  display.ssd1306_command(SSD1306_DISPLAYON);                  // 0xAF

}

void loop() {
  // 显示动态文本
  // 读取电位器的电压值 
  int analogValue = analogRead(POTENTIOMETER_PIN);
  float voltage = analogValue * (3.3 / 4095.0);// 将ADC值转换为电压值
  // 读取电位器的值
  potPercent=(float)analogValue/4095*100;
  potXValue = analogRead(potPinX);
  potYValue = analogRead(potPinY);
  Serial.print("X=");
  Serial.println(potXValue);
  Serial.print("Y=");
  Serial.println(potYValue);

// 通过串口输出电压值
  Serial.print("Voltage: ");
  Serial.print(voltage, 2);// 保留两位小数
  Serial.print(" V (Raw: ");
  Serial.print(analogValue);
  Serial.println(")");

  display.clearDisplay();
  
  // 显示文本数据
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Status:");
  if(potXValue>2100)
  {
  display.print("Forward,");
  if(potYValue>2100)
  display.print("Right");
  else if(potYValue<1950)
  display.print("Left");
  else 
  display.print("Middle");
  }
  else if(potXValue<1950)
  {
  display.print("Regress,");
  if(potYValue>2100)
  display.print("Right");
  else if(potYValue<1950)
  display.print("Left");
  else 
  display.print("Middle");
  }
  else
  display.print("Stop");
  
  display.setCursor(0, 12);
  display.print("Curr: "); 
  display.print(potXValue); 
  display.print(",");
  display.print(potYValue);
  display.print("A");
  
  display.setCursor(0, 24);
  display.print("Bat:  "); 
  display.print(batPercent); 
  display.print("%");
  
  display.setCursor(0, 36);
  display.print("Power:  "); 
  display.print(potPercent); 
  display.print("%");

  int direction_val = potYValue;
  int pot_value = analogValue;
  speed = map(pot_value, 0, 4095, 0, 255);
  int dir_pos=0,dir_neg=0;
  if(potXValue>=2100)
  dir_pos = 1;
  else if(potXValue<=1950)
 dir_neg = 1;


  // 调试输出
  Serial.print("Pot:");
  Serial.print(pot_value);
  Serial.print(" Spd:");
  Serial.print(speed);
  Serial.print(" Dir:");
  Serial.print(direction_val);
  Serial.print(" POS:");
  Serial.print(dir_pos);
  Serial.print(" NEG:");
  Serial.println(dir_neg);

  // 更新电机控制
  setMotor(speed, dir_pos, dir_neg, direction_val);
  
  // 绘制电池图标和进度条
  drawBatteryIcon(100, 24, batPercent);
  drawProgressBar(0, 48, 128, 6, batPercent); // 电池百分比进度条
  drawProgressBar(0, 58, 128, 6, potPercent); // 电位器百分比进度条
  display.display();

  delay(500);

}

// 显示文本（带自动换行）
void displayText(String text, int x, int y) {
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(x, y);
  display.print(text);
}
// 绘制电池图标
void drawBatteryIcon(int x, int y, int percent) {
  // 电池主体
  display.drawRect(x, y, 20, 10, SSD1306_WHITE);
  // 电池正极
  display.fillRect(x+20, y+3, 2, 4, SSD1306_WHITE);
  // 电量填充
  int fillWidth = map(percent, 0, 100, 0, 18);
  display.fillRect(x+1, y+1, fillWidth, 8, SSD1306_WHITE);
}
void drawProgressBar(int x, int y, int width, int height, int percent) {
  int barWidth = map(percent, 0, 100, 0, width-2);
  display.drawRect(x, y, width, height, SSD1306_WHITE);
  display.fillRect(x+1, y+1, barWidth, height-2, SSD1306_WHITE);
}
void setMotor(int speed_val, int dir_pos, int dir_neg, int steer) {
  // 限制速度范围
  speed_val = constrain(speed_val, 0, 255);
  
  // 初始化电机速度
  int left_speed = speed_val;
  int right_speed = speed_val;
  
  // 计算转向因子 (0-1)
  float steer_factor = 0;
  if (steer > 2100) {        // 左转
    steer_factor = (steer - 2100) / 1995.0;  // 4095-2100=1995
    left_speed *= (1.0 - steer_factor);
  } 
  else if (steer < 1950) {   // 右转
    steer_factor = (2100 - steer) / 1950.0;
    right_speed *= (1.0 - steer_factor);
  }

  // 应用方向控制
  if (dir_pos == LOW && dir_neg == HIGH) {  // 正转
    // 左电机正转
    ledcWrite(PWM_CH_L_L, left_speed);
    ledcWrite(PWM_CH_R_L, 0);
    // 右电机正转
    ledcWrite(PWM_CH_L_R, right_speed);
    ledcWrite(PWM_CH_R_R, 0);
  } 
  else if (dir_pos == HIGH && dir_neg == LOW) {  // 反转
    // 左电机反转
    ledcWrite(PWM_CH_L_L, 0);
    ledcWrite(PWM_CH_R_L, left_speed);
    // 右电机反转
    ledcWrite(PWM_CH_L_R, 0);
    ledcWrite(PWM_CH_R_R, right_speed);
  } 
  else {  // 停止
    ledcWrite(PWM_CH_L_L, 0);
    ledcWrite(PWM_CH_R_L, 0);
    ledcWrite(PWM_CH_L_R, 0);
    ledcWrite(PWM_CH_R_R, 0);
  }
}
